%% Start up
h=COM_OpenNXT();
COM_SetDefaultNXT(h);


%%location (xyz)


%%calibration
global angle1a;
angle1a=0;
global angle1b;
angle1b=1.4;
global angle1c;
angle1c=0;

global angle2a;
angle2a=0;
global angle2b;
angle2b=0.3782;
global angle2c;
angle2c=1.7372;

global angle3a;
angle3a=0;
global angle3b;
angle3b=1;
global angle3c;
angle3c=0;


%%location (angle)
pt1=[-36 -13 73];
%pt1(2)=round(pt1(2)*0.3782+1.7372);
%pt1(1)=round(pt1(1)*1.4);
pt2=[0 0 0];
pt3=[-36 -13 73];
pt4=[0 0 0];
pt5=[-56 -95 116];
pt6=[0 0 0];
pt7=[-56 -95 116];
pt8=[0 0 0];
pt9=[-56 -95 116];
pt10=[0 0 0];

%{
%%Inverse kinematics
startup_rvc
t = 0:2;
theta = [0, 0, 0];
d = [0.035, 0, 0];
a = [.06, .135, 0];
alpha = [-pi/2, 0, 0];
sigma = [0, 0, 0];
for i = 1:3
   
    L(i) = Link([theta(i), d(i), a(i), alpha(i)]);

end
robot_arm = SerialLink(L, 'name', 'Lego Bot');
robot_arm.tool = transl(0.177, -0.015, 0);
T1 = [ 1     0    0     .13;  %out radially
      0     1    0     .155;   %left right
      0     0    1      -.035; %up down
      0     0    0     1.0];
T2 = [ 1     0    0     0.295;  %out radially
      0     1    0     -0.125;   %left right
      0     0    1    -0.035; %up down
      0     0    0     1.0];
Ts = ctraj(T1, T2, length(t));
mask = [1 1 1 0 0 0];
qs = robot_arm.ikine(Ts,[0 0 2], mask, 'alpha', .85, 'ilimit', 800);
pt1(1)=qs(3,1);
pt1(2)=qs(3,2);
pt1(3)=qs(3,3);

%}












%% Do your stuff

myMotor1             = NXTMotor();   % create class
myMotor1.Port        = MOTOR_A;  % same as  myMotors.Port = 'BC';
myMotor1.Power       = -80;       % power range from -100 to 100
%myMotor1.TachoLimit  = 26+30;     % the number of degree of rotation  0 up to 9999 
myMotor1.SmoothStart = true;	% motor startup setting
myMotor1.ActionAtTachoLimit = 'HoldBrake';   %prevent the motor from moving during stop
myMotor1.SpeedRegulation = false;  % no regulation on speed

myMotor2             = NXTMotor();   % create class
myMotor2.Port        = MOTOR_B;  % same as  myMotors.Port = 'BC';
myMotor2.Power       = -80;       % power range from -100 to 100
%myMotor2.TachoLimit  = 75+20;     % the number of degree of rotation  0 up to 9999 
myMotor2.SmoothStart = true;	% motor startup setting
myMotor2.ActionAtTachoLimit = 'HoldBrake';   %prevent the motor from moving during stop
myMotor2.SpeedRegulation = false;  % no regulation on speed

myMotor3             = NXTMotor();   % create class
myMotor3.Port        = MOTOR_C;  % same as  myMotors.Port = 'BC';
myMotor3.Power       = 80;       % power range from -100 to 100
%myMotor3.TachoLimit  = 116;     % the number of degree of rotation  0 up to 9999 
myMotor3.SmoothStart = true;	% motor startup setting
myMotor3.ActionAtTachoLimit = 'HoldBrake';   %prevent the motor from moving during stop
myMotor3.SpeedRegulation = false;  % no regulation on speed

Power1=60;
Power2=60;
Power3=60;



%pt1 conversion to machine command
if pt1(1) >0   
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt1(1);   
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt1(1);         
end  


if pt1(2) >0
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = pt1(2);        
else   
myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = -1*pt1(2);       
end 


if pt1(3) >0  
myMotor3.Power       = Power3;       
myMotor3.TachoLimit  = pt1(3);    
else
myMotor3.Power       = -Power3;       
myMotor3.TachoLimit  = -1*pt1(3); 
end 


myMotor1.TachoLimit=round(angle1a*(myMotor1.TachoLimit)^2+angle1b*myMotor1.TachoLimit+angle1c);
myMotor2.TachoLimit=round(angle2a*(myMotor2.TachoLimit)^2+angle2b*myMotor2.TachoLimit+angle2c);
myMotor3.TachoLimit=round(angle3a*(myMotor3.TachoLimit)^2+angle3b*myMotor3.TachoLimit+angle3c);


myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();

myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();

myMotor3.Stop('off');
myMotor3.ResetPosition();
myMotor3.SendToNXT();
myMotor3.WaitFor();


pt1(1)=-1*pt1(1);
pt1(2)=-1*pt1(2);
pt1(2)=-1*pt1(2);







%{
step=(myMotor.TachoLimit)/3; 
step1=(myMotor1.TachoLimit)/3; 
step2=(myMotor2.TachoLimit)/3; 

myMotor.TachoLimit=round(step);
myMotor1.TachoLimit=round(step1);
myMotor2.TachoLimit=round(step2);

for i=1:3

    
myMotor.SendToNXT();
myMotor1.SendToNXT();
myMotor2.SendToNXT();
myMotor.WaitFor();
myMotor1.WaitFor();
myMotor2.WaitFor();
    
end   


%}




%pt2 conversion to machine command

pt2(1)=pt2(1)-pt1(1);
pt2(2)=pt2(2)-pt1(2);
pt2(3)=pt2(3)-pt1(3);



if pt2(1) >0   
myMotor.Power       = Power;       
myMotor.TachoLimit  = pt2(1);   
else   
myMotor.Power       = -Power;       
myMotor.TachoLimit  = -1*pt2(1);         
end  

if pt2(2) >0
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt2(2);        
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt2(2);       
end 

if pt2(3) >0  
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = pt2(3);    
else
myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = -1*pt2(3); 
end 

myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();


myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();


myMotor.Stop('off');
myMotor.ResetPosition();
myMotor.SendToNXT();
myMotor.WaitFor();


%{

%pt3 conversion to machine command
pt3(1)=pt3(1)-pt2(1);
pt3(2)=pt3(2)-pt2(2);
pt3(3)=pt3(3)-pt2(3);



if pt3(1) >0   
myMotor.Power       = Power;       
myMotor.TachoLimit  = pt3(1);   
else   
myMotor.Power       = -Power;       
myMotor.TachoLimit  = -1*pt3(1);         
end  

if pt3(2) >0
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt3(2);        
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt3(2);       
end 

if pt3(3) >0  
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = pt3(3);    
else
myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = -1*pt3(3); 
end 


myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();

myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();

myMotor.Stop('off');
myMotor.ResetPosition();
myMotor.SendToNXT();
myMotor.WaitFor();


%pt4 conversion to machine command
pt4(1)=pt4(1)-pt3(1);
pt4(2)=pt4(2)-pt3(2);
pt4(3)=pt4(3)-pt3(3);

if pt4(1) >0   
myMotor.Power       = Power;       
myMotor.TachoLimit  = pt4(1);   
else   
myMotor.Power       = -Power;       
myMotor.TachoLimit  = -1*pt4(1);         
end  

if pt4(2) >0
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt4(2);        
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt4(2);       
end 

if pt4(3) >0  
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = pt4(3);    
else
myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = -1*pt4(3); 
end 

myMotor.Stop('off');
myMotor.ResetPosition();
myMotor.SendToNXT();
myMotor.WaitFor();

myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();

myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();

%pt5 conversion to machine command
pt5(1)=pt5(1)-pt4(1);
pt5(2)=pt5(2)-pt4(2);
pt5(3)=pt5(3)-pt4(3);

if pt5(1) >0   
myMotor.Power       = Power;       
myMotor.TachoLimit  = pt5(1);   
else   
myMotor.Power       = -Power;       
myMotor.TachoLimit  = -1*pt5(1);         
end  

if pt5(2) >0
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt5(2);        
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt5(2);       
end 

if pt5(3) >0  
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = pt5(3);    
else
myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = -1*pt5(3); 
end 

myMotor.Stop('off');
myMotor.ResetPosition();
myMotor.SendToNXT();
myMotor.WaitFor();

myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();

myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();

%pt6 conversion to machine command
pt6(1)=pt6(1)-pt5(1);
pt6(2)=pt6(2)-pt5(2);
pt6(3)=pt6(3)-pt5(3);

if pt6(1) >0   
myMotor.Power       = Power;       
myMotor.TachoLimit  = pt6(1);   
else   
myMotor.Power       = -Power;       
myMotor.TachoLimit  = -1*pt6(1);         
end  

if pt6(2) >0
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt6(2);        
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt6(2);       
end 

if pt6(3) >0  
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = pt6(3);    
else
myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = -1*pt6(3); 
end 

myMotor.Stop('off');
myMotor.ResetPosition();
myMotor.SendToNXT();
myMotor.WaitFor();

myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();

myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();

%pt7 conversion to machine command
pt7(1)=pt7(1)-pt6(1);
pt7(2)=pt7(2)-pt6(2);
pt7(3)=pt7(3)-pt6(3);

if pt7(1) >0   
myMotor.Power       = Power;       
myMotor.TachoLimit  = pt7(1);   
else   
myMotor.Power       = -Power;       
myMotor.TachoLimit  = -1*pt7(1);         
end  

if pt7(2) >0
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt7(2);        
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt7(2);       
end 

if pt7(3) >0  
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = pt7(3);    
else
myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = -1*pt7(3); 
end 

myMotor.Stop('off');
myMotor.ResetPosition();
myMotor.SendToNXT();
myMotor.WaitFor();

myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();

myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();

%pt8 conversion to machine command
pt8(1)=pt8(1)-pt7(1);
pt8(2)=pt8(2)-pt7(2);
pt8(3)=pt8(3)-pt7(3);


if pt8(1) >0   
myMotor.Power       = Power;       
myMotor.TachoLimit  = pt8(1);   
else   
myMotor.Power       = -Power;       
myMotor.TachoLimit  = -1*pt8(1);         
end  

if pt8(2) >0
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt8(2);        
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt8(2);       
end 

if pt8(3) >0  
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = pt8(3);    
else
myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = -1*pt8(3); 
end 

myMotor.Stop('off');
myMotor.ResetPosition();
myMotor.SendToNXT();
myMotor.WaitFor();

myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();

myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();

%pt9 conversion to machine command
pt9(1)=pt9(1)-pt8(1);
pt9(2)=pt9(2)-pt8(2);
pt9(3)=pt9(3)-pt8(3);


if pt9(1) >0   
myMotor.Power       = Power;       
myMotor.TachoLimit  = pt9(1);   
else   
myMotor.Power       = -Power;       
myMotor.TachoLimit  = -1*pt9(1);         
end  

if pt9(2) >0
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt9(2);        
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt9(2);       
end 

if pt9(3) >0  
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = pt9(3);    
else
myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = -1*pt9(3); 
end 

myMotor.Stop('off');
myMotor.ResetPosition();
myMotor.SendToNXT();
myMotor.WaitFor();

myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();

myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();

%pt10 conversion to machine command
pt10(1)=pt10(1)-pt9(1);
pt10(2)=pt10(2)-pt9(2);
pt10(3)=pt10(3)-pt9(3);


if pt10(1) >0   
myMotor.Power       = Power;       
myMotor.TachoLimit  = pt10(1);   
else   
myMotor.Power       = -Power;       
myMotor.TachoLimit  = -1*pt10(1);         
end  

if pt10(2) >0
myMotor1.Power       = Power1;       
myMotor1.TachoLimit  = pt10(2);        
else   
myMotor1.Power       = -Power1;       
myMotor1.TachoLimit  = -1*pt10(2);       
end 

if pt10(3) >0  
myMotor2.Power       = Power2;       
myMotor2.TachoLimit  = pt10(3);    
else
myMotor2.Power       = -Power2;       
myMotor2.TachoLimit  = -1*pt10(3); 
end 

myMotor.Stop('off');
myMotor.ResetPosition();
myMotor.SendToNXT();
myMotor.WaitFor();

myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();

myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();

%}

%while 1










%end



    

    
  




%% end of connection
COM_CloseNXT(COM_GetDefaultNXT());